FROM ros:galactic-desktop-full

# Install additional dependencies
RUN apt-get update && apt-get install -y \
    python3-colcon-common-extensions \
    python3-colcon-mixin \
    python3-vcstool \
    python3-rosdep \
    ros-galactic-eigen3-cmake-module \
    libeigen3-dev \
    ros-galactic-tf2-geometry-msgs \
    ros-galactic-geometry2 \
    ros-galactic-rviz2 \
    ros-galactic-rviz-common \
    ros-galactic-rviz-default-plugins \
    ros-galactic-rosidl-default-generators \
    ros-galactic-rosidl-default-runtime \
    ros-galactic-ament-cmake-auto \
    ros-galactic-ament-lint-auto \
    ros-galactic-ament-lint-common \
    libboost-all-dev \
    libjsoncpp-dev \
    libopencv-dev \
    python3-numpy \
    python3-matplotlib \
    && rm -rf /var/lib/apt/lists/*

# Setup workspace
WORKDIR /workspace
COPY . /workspace/

# Initialize rosdep
RUN rosdep init || true
RUN rosdep update

# Install dependencies  
RUN rosdep install --from-paths . --ignore-src -r -y

# Build workspace
RUN /bin/bash -c "source /opt/ros/galactic/setup.bash && colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release"

# Setup entrypoint
RUN echo "source /opt/ros/galactic/setup.bash" >> ~/.bashrc
RUN echo "source /workspace/install/setup.bash" >> ~/.bashrc

ENTRYPOINT ["/bin/bash"]